/*
这段代码是一个ROS客户端程序，用于向set_rvo_goals服务发送请求，以设置多个目标点。客户端根据命令行输入参数设置目标点，并调用服务。目标点可以以“默认”模式或“随机”模式指定。
*/

#include "ros/ros.h"
#include "geometry_msgs/Point.h"
#include "rvo_ros/SetGoals.h"
#include <list>

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "set_goals_client");
    ros::NodeHandle n;

    // 创建服务客户端，连接到set_rvo_goals服务
    ros::ServiceClient client = n.serviceClient<rvo_ros::SetGoals>("set_rvo_goals");

    // 创建服务请求对象
    rvo_ros::SetGoals srv;

    // 从命令行参数获取模型类型（默认或随机）
    srv.request.model = argv[1];
    int args_num = argc - 1;       // 参数总数
    int coordinate_num = argc - 2; // 坐标参数数目

    // 从参数服务器获取goals参数并输出
    std::string goals;
    n.getParam("goals", goals);
    std::cout << goals;

    // 输出输入的参数
    std::cout << "arguments input:" << std::endl;

    // 如果没有输入参数，输出错误信息
    if (args_num == 0)
        ROS_ERROR("Please input the args");

    // 如果坐标数目不是偶数，输出错误信息
    if (coordinate_num % 2 != 0)
    {
        std::cout << "The num of coordinates is wrong" << std::endl;
        return 1;
    }

    // 如果模型类型是默认模型，解析坐标点
    if (srv.request.model == "default")
    {
        for (int i = 2; i < coordinate_num + 2; i = i + 2)
        {
            geometry_msgs::Point point;
            point.x = atof(argv[i]);
            point.y = atof(argv[i + 1]);
            srv.request.coordinates.push_back(point);
        }
    }
    // 如果模型类型是随机模型，解析坐标限制范围
    else if (srv.request.model == "random")
    {
        if (args_num < 5)
        {
            std::cout << "The num of coordinates is wrong" << std::endl;
            return 1;
        }

        geometry_msgs::Point point1;
        geometry_msgs::Point point2;

        point1.x = atof(argv[2]);
        point2.x = atof(argv[3]);
        point1.y = atof(argv[4]);
        point2.y = atof(argv[5]);

        srv.request.coordinates.push_back(point1);
        srv.request.coordinates.push_back(point2);

        std::cout << "Request has been sent" << std::endl;
        std::cout << "model: random" << std::endl;
        std::cout << "limit x" << "[" << point1.x << "," << point2.x << "]" << std::endl;
        std::cout << "limit y" << "[" << point1.y << "," << point2.y << "]" << std::endl;
    }

    // 调用服务并处理响应
    if (client.call(srv))
    {
        ROS_INFO("call service successfully, number of goals: %ld", srv.response.num_goal);
    }
    else
    {
        ROS_ERROR("call service fail");
        return 1;
    }

    return 0;
}
